/*
 ------------------------------------------------------------------------------
 Copyright (C) 2013 Eternal Games.

 This file is part of the GLQuake source code.

 The GLQuake source code is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or (at your
 option) any later version.

 The GLQuake source code is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 more details.

 You should have received a copy of the GNU General Public License along with
 the GLQuake source code; if not, write to the Free Software Foundation, Inc.,
 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 ------------------------------------------------------------------------------
*/


//
// angles.cpp - angles math library
//


#include "../shared.h"


glqAngles	angles_zero(0.0f, 0.0f, 0.0f);


/*
 ==================
 glqAngles::ToVectors
 ==================
*/
void glqAngles::ToVectors (glqVec3 *forward, glqVec3 *right, glqVec3 *up) const {

	float	sp, sy, sr, cp, cy, cr;

	glqMath::SinCos(DEG2RAD(pitch), sp, cp);
	glqMath::SinCos(DEG2RAD(yaw), sy, cy);
	glqMath::SinCos(DEG2RAD(roll), sr, cr);

	if (forward){
		forward->x = cp * cy;
		forward->y = cp * sy;
		forward->z = -sp;
	}

	if (right){
		right->x = -sr * sp * cy + cr * sy;
		right->y = -sr * sp * sy + -cr * cy;
		right->z = -sr * cp;
	}

	if (up){
		up->x = cr * sp * cy + -sr * -sy;
		up->y = cr * sp * sy + -sr * cy;
		up->z = cr * cp;
	}
}

/*
 ==================
 glqAngles::ToForward
 ==================
*/
glqVec3 glqAngles::ToForward (void) const {

	float	sp, sy, cp, cy;

	glqMath::SinCos(DEG2RAD(pitch), sp, cp);
	glqMath::SinCos(DEG2RAD(yaw), sy, cy);

	return glqVec3(cp * cy, cp * sy, -sp);
}

/*
 ==================
 glqAngles::ToRight
 ==================
*/
glqVec3 glqAngles::ToRight (void) const {

	float	sp, sy, sr, cp, cy, cr;

	glqMath::SinCos(DEG2RAD(pitch), sp, cp);
	glqMath::SinCos(DEG2RAD(yaw), sy, cy);
	glqMath::SinCos(DEG2RAD(roll), sr, cr);

	return glqVec3(-sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp);
}

/*
 ==================
 glqAngles::ToUp
 ==================
*/
glqVec3 glqAngles::ToUp (void) const {

	float	sp, sy, sr, cp, cy, cr;

	glqMath::SinCos(DEG2RAD(pitch), sp, cp);
	glqMath::SinCos(DEG2RAD(yaw), sy, cy);
	glqMath::SinCos(DEG2RAD(roll), sr, cr);

	return glqVec3(cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp);
}

/*
 ==================
 glqAngles::ToQuat
 ==================
*/
glqQuat glqAngles::ToQuat (void) const {

	float	sp, sy, sr, cp, cy, cr;
	float	srsp, srcp, crsp, crcp;

	glqMath::SinCos(DEG2RAD(pitch) * 0.5f, sp, cp);
	glqMath::SinCos(DEG2RAD(yaw) * 0.5f, sy, cy);
	glqMath::SinCos(DEG2RAD(roll) * 0.5f, sr, cr);

	srsp = sr * sp;
	srcp = sr * cp;
	crsp = cr * sp;
	crcp = cr * cp;

	return glqQuat(crsp * sy - srcp * cy, -crsp * cy - srcp * sy, srsp * cy - crcp * sy, crcp * cy + srsp * sy);
}

/*
 ==================
 glqAngles::ToMat3
 ==================
*/
glqMat3 glqAngles::ToMat3 (void) const {

	glqMat3	mat;
	float	sp, sy, sr, cp, cy, cr;

	glqMath::SinCos(DEG2RAD(pitch), sp, cp);
	glqMath::SinCos(DEG2RAD(yaw), sy, cy);
	glqMath::SinCos(DEG2RAD(roll), sr, cr);

	mat[0][0] = cp * cy;
	mat[0][1] = cp * sy;
	mat[0][2] = -sp;
	mat[1][0] = sr * sp * cy + cr * -sy;
	mat[1][1] = sr * sp * sy + cr * cy;
	mat[1][2] = sr * cp;
	mat[2][0] = cr * sp * cy + -sr * -sy;
	mat[2][1] = cr * sp * sy + -sr * cy;
	mat[2][2] = cr * cp;

	return mat;
}

/*
 ==================
 glqAngles::ToMat4
 ==================
*/
glqMat4 glqAngles::ToMat4 (void) const {

	glqMat4	mat;
	float	sp, sy, sr, cp, cy, cr;

	glqMath::SinCos(DEG2RAD(pitch), sp, cp);
	glqMath::SinCos(DEG2RAD(yaw), sy, cy);
	glqMath::SinCos(DEG2RAD(roll), sr, cr);

	mat[0][0] = cp * cy;
	mat[0][1] = sr * sp * cy + cr * -sy;
	mat[0][2] = cr * sp * cy + -sr * -sy;
	mat[0][3] = 0.0f;
	mat[1][0] = cp * sy;
	mat[1][1] = sr * sp * sy + cr * cy;
	mat[1][2] = cr * sp * sy + -sr * cy;
	mat[1][3] = 0.0f;
	mat[2][0] = -sp;
	mat[2][1] = sr * cp;
	mat[2][2] = cr * cp;
	mat[2][3] = 0.0f;
	mat[3][0] = 0.0f;
	mat[3][1] = 0.0f;
	mat[3][2] = 0.0f;
	mat[3][3] = 1.0f;

	return mat;
}

/*
 ==================
 glqAngles::ToString

 This is just a convenience function for printing angles
 ==================
*/
const char *glqAngles::ToString (int precision) const {

	return Str_FromFloatArray(ToFloatPtr(), 3, precision);
}